/*
 * sdmmc.c
 *
 *  Created on: Oct 16, 2013
 *      Author: Ken Arok
 */

#include "sdmmc_hal.h"
#include "sdmmc_spi.h"
#include "sdmmc.h"

#ifdef DEBUG
#include <stdio.h>
#include "print_debug.h"
#include "config_debug.h"

#if DEBUG_SDMMC_RAW
static char _buf_dbg[100];
#define SDMMC_RAW_DEBUGF(...)	do {	\
	snprintf(_buf_dbg, 100, __VA_ARGS__); \
	print_dbg((const char*)_buf_dbg);	\
}while(0);
#else
#define SDMMC_RAW_DEBUGF(...)
#endif /* DEBUG_SDMMC_RAW */
#endif /* DEBUG */

#ifdef SDIO_SUPPORT_ENABLE
#define IS_SDIO()  (sdmmc_card.type & CARD_TYPE_SDIO)
#else
#define IS_SDIO()  false
#endif /* SDIO_SUPPORT_ENABLE */


/* This SD MMC stack supports only the high voltage. */
#define SD_MMC_VOLTAGE_SUPPORT	(OCR_VDD_27_28 | OCR_VDD_28_29 | OCR_VDD_29_30 | OCR_VDD_30_31 | OCR_VDD_31_32 | OCR_VDD_32_33)

/* SD/MMC card states. */
enum card_state {
	SD_MMC_CARD_STATE_READY    = 0, //!< Ready to use
	SD_MMC_CARD_STATE_DEBOUNCE = 1, //!< Debounce on going
	SD_MMC_CARD_STATE_INIT     = 2, //!< Initialization on going
	SD_MMC_CARD_STATE_UNUSABLE = 3, //!< Unusable card
	SD_MMC_CARD_STATE_NO_CARD  = 4, //!< No SD/MMC card inserted
};

/* SD/MMC card information structure. */
typedef struct {
	uint32_t clock;					//!< Card access clock
	uint32_t capacity;				//!< Card capacity in KBytes
	enum card_state state;			//!< Card state
	card_type_t type;				//!< Card type
	card_version_t version;			//!< Card version
	uint8_t csd[CSD_REG_BSIZE];		//!< CSD register
} sdmmc_card_t;

static sdmmc_card_t sdmmc_card;

/* Number of block to read or write on the current transfer. */
static uint16_t sdmmc_nb_block_to_tranfer = 0;
/* Number of block remaining to read or write on the current transfer. */
static uint16_t sdmmc_nb_block_remaining = 0;

//! SD/MMC transfer rate unit codes (10K) list
const uint32_t sdmmc_trans_units[7] = {10, 100, 1000, 10000, 0, 0, 0};
//! SD transfer multiplier factor codes (1/10) list
const uint32_t sd_trans_multipliers[16] = {
	0, 10, 12, 13, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 70, 80
};
//! MMC transfer multiplier factor codes (1/10) list
const uint32_t mmc_trans_multipliers[16] = {
	0, 10, 12, 13, 15, 20, 26, 30, 35, 40, 45, 52, 55, 60, 70, 80
};

/**\brief send more than 74 clock cycles to card.
 *
 * \param _cs_is_high	Status of CS line.
 */
static void sdmmc_send_dummy_clock(bool _cs_is_high);

/** \brief Send command to card.
 *
 * \param _cmd		Command index.
 * \param _arg		Argument of the command.
 *
 * \return R1 response.
 *
 */
static uint8_t sdmmc_send_cmd(uint8_t _cmd, uint32_t _arg);

/** \brief Get command response R2.
 *
 * \return R3 response.
 *
 */
static uint16_t sdmmc_get_response_r2(void);

/** \brief Get command response R3 (OCR register).
 *
 * \return R3 response (OCR Register).
 *
 */
static uint32_t sdmmc_get_response_r3(void);

/** \brief Get command response R7.
 *
 * \return R7 response.
 *
 */
static uint32_t sdmmc_get_response_r7(void);

/** \brief Read CID register.
 *
 * \param _pbuf		Pointer to the buffer to copy CID register.
 *
 */
//static void sdmmc_get_cid_register(uint8_t *_pbuf);

/** \brief Read CSD register.
 *
 * \param _pbuf		Pointer to the buffer to copy CSD register.
 *
 */
static void sdmmc_get_csd_register(uint8_t *_pbuf);

/** \brief Read SCR register.
 *
 * \param _pbuf		Pointer to the buffer to copy SCR register.
 *
 */
static void sdmmc_get_scr_register(uint8_t *_pbuf);

/** \brief Read blocks of data from card.
 *
 * \param _dest		Pointer to buffer where data will be copied.
 * \param _nb_block	Number block of data want to be read.
 *
 * \return 0 if success. Unless error.
 *
 */
static uint8_t sdmmc_read_block(void *_dest, uint16_t _nb_block);

/**
 * \brief Write blocks data to card.
 *
 * \param _src		Pointer to buffer of data want transfer.
 * \param _nb_block	Number block of data want to be transfer.
 *
 * \return 0 if success. Unless error.
 */
static uint8_t sdmmc_write_block(const void *_src, uint16_t _nb_block);

/**
 * \brief Read a word of data from card.
 *
 * \param	Pointer to buffer where data will be copied.
 *
 * \return 0 if success. Unless error.
 *
 */
static uint8_t sdmmc_read_dword(uint32_t *_value);

/**
 * \brief Sends operation condition command and read OCR (SPI only)
 * - CMD1 sends operation condition command
 * - CMD58 reads OCR
 *
 * \return 0 if success, otherwise error.
 */
static uint8_t mmc_spi_op_cond(void);

/** \brief Ask to all cards to send their operations conditions (SPI only).
 * - ACMD41 sends operation condition command.
 * - CMD58 reads OCR
 *
 * \param _ver2   Shall be 1 if it is a SD card V2
 *
 * \return 0 if success, otherwise error.
 */
static uint8_t sd_spi_op_cond(uint8_t _ver2);


/** \brief Try to get the SDIO card's operating condition
 * - CMD5 to read OCR NF field
 * - CMD5 to wait OCR power up busy
 * - CMD5 to read OCR MP field
 *   sd_mmc_card.type is updated
 *
 * \return 0 if success, otherwise error.
 */
static uint8_t sdio_op_cond(void);

/** \brief Get SDIO max transfer speed in Hz.
 * - CMD53 reads CIS area address in CCCR area.
 * - Nx CMD53 search Fun0 tuple in CIS area
 * - CMD53 reads TPLFE_MAX_TRAN_SPEED in Fun0 tuple
 * - Compute maximum speed of SDIO
 *   and update sd_mmc_card.clock
 *
 * \return 0 if success, otherwise error.
 */
static uint8_t sdio_get_max_speed(void);

/** \brief CMD52 for SDIO - Enable the high speed mode
 * \note sdmmc_card.clock is updated.
 *
 * \return 0 if success, otherwise error.
 */
//static uint8_t sdio_cmd52_set_high_speed(void);


/** \brief CMD8 for SD card - Send Interface Condition Command.
 *
 * \note
 * Send SD Memory Card interface condition, which includes host supply
 * voltage information and asks the card whether card supports voltage.
 * Should be performed at initialization time to detect the card type.
 *
 * \param _ver2 Pointer to v2 flag to update
 *
 * \return 0 if success, otherwise error.
 */
static uint8_t sd_cmd8(uint8_t *_ver2);

/** \brief CMD8 - The card sends its EXT_CSD register as a block of data.
 *
 * \param _b_authorize_high_speed	Pointer to update with the high speed support information
 *
 * \return true if success, otherwise false
 */
static uint8_t mmc_cmd8(uint8_t *_b_authorize_high_speed);

/** \brief CMD9: Addressed card sends its card-specific
 * data (CSD) on the CMD line spi.
 *
 * \return true if success, otherwise false
 */
static bool sdmmc_cmd9_spi(void);

/** \brief Decodes MMC CSD register
 */
static void mmc_decode_csd(void);

/** \brief Decodes SD CSD register
 */
static void sd_decode_csd(void);

/**
 * \brief CMD13 - Addressed card sends its status register.
 * This function waits the clear of the busy flag
 *
 * \return true if success, otherwise false
 */
static uint8_t sdmmc_cmd13(void);

#ifdef SDIO_SUPPORT_ENABLE
/** \brief CMD52 - SDIO IO_RW_DIRECT command
 *
 * \param rw_flag   Direction, 1:write, 0:read.
 * \param func_nb   Number of the function.
 * \param rd_after_wr Read after Write flag.
 * \param reg_addr  register address.
 * \param io_data   Pointer to input argument and response buffer.
 *
 * \return 0 if success, otherwise error.
 */
static uint8_t sdio_cmd52(uint8_t _rw_flag, uint8_t _func_nb, uint32_t _reg_addr, uint8_t _rd_after_wr, uint8_t *_io_data);

/**
 * \brief CMD53 - SDIO IO_RW_EXTENDED command
 * This implementation support only the SDIO multi-byte transfer mode which is
 * similar to the single block transfer on memory.
 * Note: The SDIO block transfer mode is optional for SDIO card.
 *
 * \param _rw_flag   Direction, 1:write, 0:read.
 * \param _func_nb   Number of the function.
 * \param _reg_addr  Register address.
 * \param _inc_addr  1:Incrementing address, 0: fixed.
 * \param _size      Transfer data size.
 * \param _access_block  true, if the block access (DMA) is used
 *
 * \return 0 if success, otherwise error.
 */
static uint8_t sdio_cmd53(uint8_t _rw_flag, uint8_t _func_nb, uint32_t _reg_addr, uint8_t _inc_addr, uint32_t _size, bool _access_block);

#endif /* SDIO_SUPPORT_ENABLE */

/** \brief ACMD51 - Read the SD Configuration Register.
 *
 * \note
 * SD Card Configuration Register (SCR) provides information on the SD Memory
 * Card's special features that were configured into the given card. The size
 * of SCR register is 64 bits.
 *
 *
 * \return 0 if success, otherwise error.
 */
static uint8_t sd_acmd51(void);

/** \brief Check the card.
 *
 * \retval SD_MMC_ERR_SLOT     Wrong slot number
 * \retval SD_MMC_ERR_NO_CARD  No card present on slot
 * \retval SD_MMC_ERR_UNUSABLE Unusable card
 * \retval SD_MMC_INIT_ONGOING Card initialization requested
 * \retval SD_MMC_OK           Card present
 */
static sdmmc_error_t sdmmc_check_card(void);

/** \brief Setup the configuration card.
 */
static void sdmmc_setup_card(uint32_t _clock);

/** \brief Initialize the SD card in SPI mode.
 *
 * \note
 * This function runs the initialization procedure and the identification
 * process, then it sets the SD/MMC card in transfer state.
 * At last, it will automatically enable maximum bus width and transfer speed.
 *
 * \return SD/MMC status.
 */
static sdmmc_error_t sdmmc_card_init(void);

/**
 * \brief Initialize the MMC card in SPI mode.
 *
 * \note
 * This function runs the initialization procedure and the identification
 * process, then it sets the SD/MMC card in transfer state.
 * At last, it will automatically enable maximum bus width and transfer speed.
 *
 * \return SD/MMC status.
 */
static sdmmc_error_t sdmmc_install_mmc(void);

/* ------------------------------------------------------------------------------------------------ */
static void sdmmc_send_dummy_clock(bool _cs_is_high)
{
	if(_cs_is_high == false) {
		sdmmc_spi_deselect_device();
	}

	sdmmc_spi_send_clock();
}


static uint8_t sdmmc_send_cmd(uint8_t _cmd, uint32_t _arg)
{
	return sdmmc_spi_send_cmd(_cmd, _arg);
}


static uint16_t sdmmc_get_response_r2(void)
{
	uint32_t _tmp;
	uint16_t _r2;

	_tmp = sdmmc_spi_get_response();
	_r2 = _tmp >> 16;
	return _r2;
}


static uint32_t sdmmc_get_response_r3(void)
{
	return sdmmc_spi_get_response();
}


static uint32_t sdmmc_get_response_r7(void)
{
	return sdmmc_spi_get_response();
}

/*
static void sdmmc_get_cid_register(uint8_t *_pbuf)
{
	sdmmc_spi_get_cid_csd_register(_pbuf);
}
*/

static void sdmmc_get_csd_register(uint8_t *_pbuf)
{
	sdmmc_spi_get_cid_csd_register(_pbuf);
}


static void sdmmc_get_scr_register(uint8_t *_pbuf)
{
	sdmmc_spi_get_scr_register(_pbuf);
}


static uint8_t sdmmc_read_block(void *_dest, uint16_t _nb_block)
{
	return sdmmc_spi_read_block(_dest, _nb_block);
}


static uint8_t sdmmc_write_block(const void *_src, uint16_t _nb_block)
{
	return sdmmc_spi_write_block(_src, _nb_block);
}

/** \brief Wait end of write block.
 *
 * \return Status.
 *	\retval	true	Success.
 *	\retval false	Failed.
 *
 */
static uint8_t sdmmc_wait_end_of_write_block(void)
{
	return sdmmc_spi_wait_end_of_write_block();
}


static uint8_t sdmmc_read_dword(uint32_t *_value)
{
	return sdmmc_spi_read_dword(_value);
}


static uint8_t mmc_spi_op_cond(void)
{
	uint32_t _retry, _r7;
	uint8_t _r1;

	/*
	 * Timeout 1s = 400KHz / ((6+1)*8) cylces = 7150 retry
	 * 6 = cmd byte size
	 * 1 = response byte size
	 */
	_retry = 7150;
	do {
		_r1 = sdmmc_send_cmd(SDMMC_SPI_CMD1_SEND_OP_COND, 0);
		if (_r1 > R1_SPI_IDLE) {
			return 1;
		}

		if(_r1 == R1_SPI_IDLE) {
			/* Card ready. */
			break;
		}

		_retry--;
		if (_retry == 0) {
			return 3;
		}
	}while(1);

	/* Read OCR for SPI mode. */
	_r1 = sdmmc_send_cmd(SDMMC_SPI_CMD58_READ_OCR, OCR_CCS);
	if(_r1) {
		if(_r1 > R1_SPI_IDLE) {
			SDMMC_RAW_DEBUGF("CMD58 CSS R1: 0x%X\n", _r1);
			return 2;
		}
		_r1 = sdmmc_send_cmd(SDMMC_SPI_CMD58_READ_OCR, 0);
		if(_r1 > R1_SPI_IDLE) {
			SDMMC_RAW_DEBUGF("CMD58 R1: 0x%X\n", _r1);
			return 4;
		}
	}

	/* Check OCR value. */
	_r7 = sdmmc_get_response_r7();
	if ((_r7 & OCR_ACCESS_MODE_MASK) == OCR_ACCESS_MODE_SECTOR) {
		sdmmc_card.type |= CARD_TYPE_HC;
	}

	SDMMC_RAW_DEBUGF("Card type: %d\n", sdmmc_card.type);
	return 0;
}


static uint8_t sd_spi_op_cond(uint8_t _ver2)
{
	uint32_t _arg, _r3, _retry;
	uint8_t _r1;

	/*
	 * Timeout 1s = 400KHz / ((6+1)*8) cylces = 7150 retry
	 * 6 = cmd byte size
	 * 1 = response byte size
	 */
	_retry = 7150;
	do {
		/* CMD55 - Indicate to the card that the next command is an
		   application specific command rather than a standard command. */
		_r1 = sdmmc_send_cmd(SDMMC_CMD55_APP_CMD, 0);
		if(_r1 > R1_SPI_IDLE) {
			SDMMC_RAW_DEBUGF("CMD55 R1: 0x%X\n", _r1);
			return 1;
		}

		/* (ACMD41) Sends host OCR register. */
		_arg = 0;
		if(_ver2) {
			_arg |= SD_ACMD41_HCS;
		}
		/* Check response. */
		_r1 = sdmmc_send_cmd(SD_SPI_ACMD41_SD_SEND_OP_COND, _arg);
		if(_r1 > R1_SPI_IDLE) {
			SDMMC_RAW_DEBUGF("ACMD41 R1: 0x%X\n", _r1);
			return 2;
		}

		if(_r1 == R1_SPI_OK) {
			/*  Card is ready. */
			break;
		}

		_retry--;
		if (_retry == 0) {
			return 3;
		}
	}while(1);

	/* Read OCR for SPI mode. */
	_r1 = sdmmc_send_cmd(SDMMC_SPI_CMD58_READ_OCR, 0);
	if(_r1) {
		SDMMC_RAW_DEBUGF("CMD58 R1: 0x%X\n", _r1);
		return 4;
	}
	_r3 = sdmmc_get_response_r3();
	if((_r3 & OCR_CCS) != 0) {
		sdmmc_card.type |= CARD_TYPE_HC;
	}

	return 0;
}

#ifdef SDIO_SUPPORT_ENABLE

static uint8_t sdio_op_cond(void)
{
	uint32_t _r3;
	uint8_t _r1;
	uint32_t _cmd5_retry = 5000;

	/* CMD5 - SDIO send operation condition (OCR) command. */
	_r1 = sdmmc_send_cmd(SDIO_CMD5_SEND_OP_COND, 0);
	if (_r1 > R1_SPI_IDLE) {
		return 0; // No error but card type not updated
	}
	_r3 = sdmmc_get_response_r3();
	if ((_r3 & OCR_SDIO_NF) == 0) {
		return 0; // No error but card type not updated
	}

	/*
	 * Wait card ready
	 * Timeout 1s = 400KHz / ((6+4)*8) cylces = 5000 retry
	 * 6 = cmd byte size
	 * 4(SPI) 6(MCI) = response byte size
	 */

	while(1) {
		/* CMD5 - SDIO send operation condition (OCR) command. */
		_r1 = sdmmc_send_cmd(SDIO_CMD5_SEND_OP_COND, _r3 & SD_MMC_VOLTAGE_SUPPORT);
		if(_r1) {
			return 1;
		}
		_r3 = sdmmc_get_response_r3();
		if((_r3 & OCR_POWER_UP_BUSY) == OCR_POWER_UP_BUSY) {
			break;
		}

		if(_cmd5_retry-- == 0) {
			return 3;
		}
	}

	/* Update card type at the end of busy. */
	if ((_r7 & OCR_SDIO_MP) > 0) {
		sdmmc_card.type = CARD_TYPE_SD_COMBO;
	}
	else {
		sdmmc_card.type = CARD_TYPE_SDIO;
	}
	return 0; // No error and card type updated with SDIO type
}


static uint8_t sdio_get_max_speed(void)
{
	uint32_t _addr, _addr_cis;
	uint8_t _buf[6];
 	uint32_t _unit;
	uint32_t _mul;
	uint8_t _tplfe_max_tran_speed, _retval;

	/* Read CIS area address in CCCR area. */
	_addr_cis = 0; // Init all bytes, because the next function fill 3 bytes only
	_retval = sdio_cmd53(SDIO_CMD53_READ_FLAG, SDIO_CIA, SDIO_CCCR_CIS_PTR, 1, 3, true);
	if(_retval) {
		return _retval;
	}

	_retval = sdmmc_read_block((uint8_t *)&_addr_cis, 1);
	if(_retval) {
		return _retval;
	}

	_addr_cis = le32_to_cpu(_addr_cis);

	/* Search Fun0 tuple in the CIA area. */
	_addr = _addr_cis;
	while(1) {
		/* Read a sample of CIA area. */
		_retval = sdio_cmd53(SDIO_CMD53_READ_FLAG, SDIO_CIA, _addr, 1, 3, true);
		if(_retval) {
			return _retval;
		}

		_retval = sdmmc_read_block(_buf, 1);
		if(_retval) {
			return _retval;
		}

		if(_buf[0] == SDIO_CISTPL_END) {
			return 11; // Tuple error
		}
		if(_buf[0] == SDIO_CISTPL_FUNCE && _buf[2] == 0x00) {
			break; // Fun0 tuple found
		}
		if (_buf[1] == 0) {
			return 12; // Tuple error
		}

		/* Next address. */
		_addr += (_buf[1] + 2);
		if(_addr > (_addr_cis + 256)) {
			return 13; // Outoff CIS area
		}
	}

	/* Read all Fun0 tuple fields: fn0_blk_siz & max_tran_speed. */
	_retval = sdio_cmd53(SDIO_CMD53_READ_FLAG, SDIO_CIA, _addr, 1, 6, true);
	if(_retval) {
		return _retval;
	}

	_retval = sd_mmc_read_block(_buf, 1);
	if(_retval) {
		return _retval;
	}

	_tplfe_max_tran_speed = buf[5];
	if (_tplfe_max_tran_speed > 0x32) {
		/* Error on SDIO register, the high speed is not activated
		 * and the clock can not be more than 25MHz.
		 * This error is present on specific SDIO card
		 * (H&D wireless card - HDG104 WiFi SIP).
		 */
		_tplfe_max_tran_speed = 0x32; // 25Mhz
	}

	/* Decode transfer speed in Hz. */
	_unit = sdmmc_trans_units[_tplfe_max_tran_speed & 0x7];
	_mul = sdtrans_multipliers[(_tplfe_max_tran_speed >> 3) & 0xF];
	sdmmc_card.clock = _unit * _mul * 1000;
	/**
	 * Note: A combo card shall be a Full-Speed SDIO card
	 * which supports upto 25MHz.
	 * A SDIO card alone can be:
	 * - a Low-Speed SDIO card which supports 400Khz minimum
	 * - a Full-Speed SDIO card which supports upto 25MHz
	 */
	return 0;
}


static uint8_t sdio_cmd52_set_high_speed(void)
{
	uint8_t _u8_value, _retval;

	/* Check CIA.HS */
	_retval = sdio_cmd52(SDIO_CMD52_READ_FLAG, SDIO_CIA, SDIO_CCCR_HS, 0, &u8_value);
	if(_retval) {
		return _retval;
	}

	if ((u8_value & SDIO_SHS) != SDIO_SHS) {
		/* No supported, it is not a protocol error. */
		return 0;
	}
	/* HS mode possible, then enable. */
	_u8_value = SDIO_EHS;
	_retval = sdio_cmd52(SDIO_CMD52_WRITE_FLAG, SDIO_CIA, SDIO_CCCR_HS, 1, &u8_value);
	if (_retval) {
		return _retval;
	}
	sdmmc_card.clock = 25000000lu;
	return _retval;
}

#else /* SDIO_SUPPORT_ENABLE */

static uint8_t sdio_op_cond(void)
{
	return 3;
}

static uint8_t sdio_get_max_speed(void)
{
	return 3;
}


#endif /* SDIO_SUPPORT_ENABLE */


static uint8_t sd_cmd8(uint8_t *_ver2)
{
	uint8_t _r1;
	uint32_t _r7;

	*_ver2 = 0;
	/* Test for SD version 2. */
	_r1 = sdmmc_send_cmd(SD_CMD8_SEND_IF_COND, SD_CMD8_PATTERN | SD_CMD8_HIGH_VOLTAGE);

	if(_r1 & R1_SPI_ERROR) {
		/* Error SPI. */
		SDMMC_RAW_DEBUGF("CMD8 R1: 0x%X\n", _r1);
		return 1;
	}

	if(_r1 != R1_SPI_IDLE) {
		/* It is not a Version 2. */
		SDMMC_RAW_DEBUGF("CMD8 R1: 0x%X\n", _r1);
		return 0;
	}

	/* Check R7 response. */
	_r7 = sdmmc_get_response_r7();
	if (_r7 == 0xFFFFFFFF) {
		/* No compliance R7 value. */
		SDMMC_RAW_DEBUGF("CMD8 R7: 0x%X\n", (int)_r7);
		return 0; // It is not a V2
	}

	if ((_r7 & (SD_CMD8_MASK_PATTERN | SD_CMD8_MASK_VOLTAGE)) != (SD_CMD8_PATTERN | SD_CMD8_HIGH_VOLTAGE)) {
		SDMMC_RAW_DEBUGF("CMD8 R7: 0x%X\n", (int)_r7);
		return 2;
	}

	*_ver2 = 1;
	return 0;
}


static uint8_t mmc_cmd8(uint8_t *_b_authorize_high_speed)
{
	uint8_t _r1;
	uint16_t i;
	uint32_t _ext_csd;
	uint32_t _sec_count;

	_r1 = sdmmc_send_cmd(MMC_CMD8_SEND_EXT_CSD, 0);
	if (_r1) {
		SDMMC_RAW_DEBUGF("CMD8 R1: 0x%X\n", _r1);
		return 1;
	}
	/** Read and decode Extended Extended CSD
		Note: The read access is done in byte to avoid a buffer
		of EXT_CSD_BSIZE Byte in stack. */

	/* Read card type. */
	for (i = 0; i < (EXT_CSD_CARD_TYPE_INDEX + 4) / 4; i++) {
		if (sdmmc_read_dword(&_ext_csd)) {
			return 2;
		}
	}

	*_b_authorize_high_speed = (_ext_csd >> ((EXT_CSD_CARD_TYPE_INDEX % 4) * 8)) & MMC_CTYPE_52MHZ;

	if (MMC_CSD_C_SIZE(sdmmc_card.csd) == 0xFFF) {
		/* For high capacity SD/MMC card,
		   memory capacity = SEC_COUNT * 512 byte. */
		for (; i <(EXT_CSD_SEC_COUNT_INDEX + 4) / 4; i++) {
			if (sdmmc_read_dword(&_sec_count)) {
				return 4;
			}
		}
		sdmmc_card.capacity = _sec_count / 2;
	}
	for (; i < EXT_CSD_BSIZE / 4; i++) {
		if (sdmmc_read_dword(&_sec_count)) {
			return 5;
		}
	}
	return 0;
}


static bool sdmmc_cmd9_spi(void)
{
	uint8_t _r1;

	_r1 = sdmmc_send_cmd(SDMMC_SPI_CMD9_SEND_CSD, 0);

	if(_r1) {
		SDMMC_RAW_DEBUGF("CMD9 R1: 0x%X\n", _r1);
		return 1;
	}

	sdmmc_get_csd_register(sdmmc_card.csd);

	return 0;
}


static void mmc_decode_csd(void)
{
 	uint32_t _unit;
	uint32_t _mul;
	uint32_t _tran_speed;

	/* Get MMC System Specification version supported by the card. */
	switch (MMC_CSD_SPEC_VERS(sdmmc_card.csd)) {
		case 0:
			sdmmc_card.version = CARD_VER_MMC_1_2;
		break;

		case 1:
			sdmmc_card.version = CARD_VER_MMC_1_4;
		break;

		case 2:
			sdmmc_card.version = CARD_VER_MMC_2_2;
		break;

		case 3:
			sdmmc_card.version = CARD_VER_MMC_3;
		break;

		case 4:
			sdmmc_card.version = CARD_VER_MMC_4;
		break;

		default: break;
	}

	/* Get MMC memory max transfer speed in Hz. */
	_tran_speed = CSD_TRAN_SPEED(sdmmc_card.csd);
	_unit = sdmmc_trans_units[_tran_speed & 0x7];
	_mul = mmc_trans_multipliers[(_tran_speed >> 3) & 0xF];
	sdmmc_card.clock = _unit *_mul * 1000;

	if(sdmmc_card.clock > 25000000lu) {
		sdmmc_card.clock = 25000000lu;
	}

	/*
	 * Get card capacity.
	 * ----------------------------------------------------
	 * For normal SD/MMC card:
	 * memory capacity = BLOCKNR * BLOCK_LEN
	 * Where
	 * BLOCKNR = (C_SIZE+1) * MULT
	 * MULT = 2 ^ (C_SIZE_MULT+2)       (C_SIZE_MULT < 8)
	 * BLOCK_LEN = 2 ^ READ_BL_LEN      (READ_BL_LEN < 12)
	 * ----------------------------------------------------
	 * For high capacity SD/MMC card:
	 * memory capacity = SEC_COUNT * 512 byte
	 */
	if (MMC_CSD_C_SIZE(sdmmc_card.csd) != 0xFFF) {
		uint32_t blocknr = ((MMC_CSD_C_SIZE(sdmmc_card.csd) + 1) * (1 << (MMC_CSD_C_SIZE_MULT(sdmmc_card.csd) + 2)));
		sdmmc_card.capacity = blocknr * (1 << MMC_CSD_READ_BL_LEN(sdmmc_card.csd)) / 1024;
	}
}


static void sd_decode_csd(void)
{
 	uint32_t _unit;
	uint32_t _mul;
	uint32_t _tran_speed;

	// Get SD memory maximum transfer speed in Hz.
	_tran_speed = CSD_TRAN_SPEED(sdmmc_card.csd);
	_unit = sdmmc_trans_units[_tran_speed & 0x7];
	_mul = sd_trans_multipliers[(_tran_speed >> 3) & 0xF];
	sdmmc_card.clock = _unit * _mul * 1000;

	if(sdmmc_card.clock > 25000000lu) {
		sdmmc_card.clock = 25000000lu;
	}

	/*
	 * Get card capacity.
	 * ----------------------------------------------------
	 * For normal SD/MMC card:
	 * memory capacity = BLOCKNR * BLOCK_LEN
	 * Where
	 * BLOCKNR = (C_SIZE+1) * MULT
	 * MULT = 2 ^ (C_SIZE_MULT+2)       (C_SIZE_MULT < 8)
	 * BLOCK_LEN = 2 ^ READ_BL_LEN      (READ_BL_LEN < 12)
	 * ----------------------------------------------------
	 * For high capacity SD card:
	 * memory capacity = (C_SIZE+1) * 512K byte
	 */
	if (CSD_STRUCTURE_VERSION(sdmmc_card.csd) >= SD_CSD_VER_2_0) {
		sdmmc_card.capacity = (SD_CSD_2_0_C_SIZE(sdmmc_card.csd) + 1) * 512;
	}
	else {
		uint32_t blocknr = ((SD_CSD_1_0_C_SIZE(sdmmc_card.csd) + 1) * (1 << (SD_CSD_1_0_C_SIZE_MULT(sdmmc_card.csd) + 2)));
		sdmmc_card.capacity = blocknr * (1 << SD_CSD_1_0_READ_BL_LEN(sdmmc_card.csd)) / 1024;
	}
}


static uint8_t sdmmc_cmd13(void)
{
	uint32_t _nec_timeout;
	uint16_t _r2;
	uint8_t _r1;

	/* Wait for data ready status.
	 * Nec timing: 0 to unlimited
	 * However a timeout is used.
	 * 200 000 * 8 cycles
	 */
	_nec_timeout = 200000;
	do {
		_r1 = sdmmc_spi_send_cmd(SDMMC_SPI_CMD13_SEND_STATUS, 0);
		if (_r1) {
			SDMMC_RAW_DEBUGF("CMD13 R1: 0x%X\n", _r1);
			return 1;
		}
		/* Check busy flag. */
		_r2 = sdmmc_get_response_r2();
		_r2 = _r2 >> 8;
		if (!(_r2 & 0x00FF)) {
			break;
		}
		_nec_timeout--;
		if (_nec_timeout == 0) {
			return 3;
		}
	} while(1);

	return 0;
}


#ifdef SDIO_SUPPORT_ENABLE

static uint8_t sdio_cmd52(uint8_t _rw_flag, uint8_t _func_nb, uint32_t _reg_addr, uint8_t _rd_after_wr, uint8_t *_io_data)
{
	uint8_t _r1;

	_r1 = sdmmc_spi_send_cmd(SDIO_CMD52_IO_RW_DIRECT, ((uint32_t)*_io_data << SDIO_CMD52_WR_DATA)
								| ((uint32_t)_rw_flag << SDIO_CMD52_RW_FLAG)
								| ((uint32_t)_func_nb << SDIO_CMD52_FUNCTION_NUM)
								| ((uint32_t)_rd_after_wr << SDIO_CMD52_RAW_FLAG)
								| ((uint32_t)_reg_addr << SDIO_CMD52_REG_ADRR)));
	if(_r1) {
		return 1;
	}

	*_io_data = sdmmc_get_response_r2() & 0xFF;
	return 0;
}


static uint8_t sdio_cmd53(uint8_t _rw_flag, uint8_t _func_nb, uint32_t _reg_addr, uint8_t _inc_addr, uint32_t _size, bool _access_block)
{
	uint8_t _r1;

	if (size == 512) {
		size = 0; // 0 = 512B on SDIO command
	}

	_r1 = sdmmc_send_cmd((_rw_flag == SDIO_CMD53_READ_FLAG) ? SDIO_CMD53_IO_R_BYTE_EXTENDED : SDIO_CMD53_IO_W_BYTE_EXTENDED,
							(_size << SDIO_CMD53_COUNT)
							| ((uint32_t)_reg_addr << SDIO_CMD53_REG_ADDR)
							| ((uint32_t)_inc_addr << SDIO_CMD53_OP_CODE)
							| ((uint32_t)0 << SDIO_CMD53_BLOCK_MODE)
							| ((uint32_t)_func_nb << SDIO_CMD53_FUNCTION_NUM)
							| ((uint32_t)_rw_flag << SDIO_CMD53_RW_FLAG),
							_size, 1, _access_block);

	if(_r1) {
		return 1;
	}

	return 0;

}
#endif /* SDIO_SUPPORT_ENABLE */


static uint8_t sd_acmd51(void)
{
	uint8_t _scr[SD_SCR_REG_BSIZE];
	uint8_t _r1, i = 0;

	/* CMD55 - Indicate to the card that the next command is an
	   application specific command rather than a standard command. */
	do {
		_r1 = sdmmc_send_cmd(SDMMC_CMD55_APP_CMD, 0);
		i++;
	}while(_r1 && (i < 20));

	if (_r1) {
		SDMMC_RAW_DEBUGF("CMD55 R1: 0x%X\n", _r1);
		return 1;
	}

	_r1 = sdmmc_send_cmd(SD_ACMD51_SEND_SCR, 0);
	if (_r1) {
		SDMMC_RAW_DEBUGF("ACMD51 R1: 0x%X\n", _r1);
		return 2;
	}

	sdmmc_get_scr_register(_scr);

	/* Get SD Memory Card - Spec. Version */
	switch (SD_SCR_SD_SPEC(_scr)) {
		case SD_SCR_SD_SPEC_1_0_01:
			sdmmc_card.version = CARD_VER_SD_1_0;
		break;

		case SD_SCR_SD_SPEC_1_10:
			sdmmc_card.version = CARD_VER_SD_1_10;
		break;

		case SD_SCR_SD_SPEC_2_00:
			if(SD_SCR_SD_SPEC3(_scr) == SD_SCR_SD_SPEC_3_00) {
				sdmmc_card.version = CARD_VER_SD_3_0;
			}
			else {
				sdmmc_card.version = CARD_VER_SD_2_0;
			}
		break;

		default:
			sdmmc_card.version = CARD_VER_SD_1_0;
		break;
	}

	return 0;
}


static sdmmc_error_t sdmmc_check_card(void)
{
	uint8_t _r1;
	uint8_t i = 100;

	/* No pin card detection, then read CID register. */
	do {
		_r1 = sdmmc_send_cmd(SDMMC_SPI_CMD16_ILLEGAL_CMD, 0);
		i--;
	}while(((_r1 == 0xFF) || (_r1 == 0x00)) && i);

	SDMMC_RAW_DEBUGF("Check CMD16 R1: 0x%X\n", _r1);

	if(i == 0) {
		/* Card is not initialized. */
		sdmmc_card.state = SD_MMC_CARD_STATE_INIT;
		/* Low clock for initialization. */
		sdmmc_card.clock = SDMMC_CLOCK_INIT;
	}

	return (sdmmc_card.state == SD_MMC_CARD_STATE_INIT) ? SD_MMC_INIT_ONGOING : SD_MMC_OK;
}


static void sdmmc_setup_card(uint32_t _clock)
{
	sdmmc_spi_setup_device(_clock);
}


static sdmmc_error_t sdmmc_card_init(void)
{
	uint8_t _ver2 = 0;
	uint8_t _r1, _retry;

	SDMMC_RAW_DEBUGF("Card Initialize...\n");

	/* In first, try to install SD/SDIO card. */
	sdmmc_card.type = CARD_TYPE_SD;
	sdmmc_card.version = CARD_VER_UNKNOWN;

	/* Configure the card interface for initialization. */
	sdmmc_setup_card(SDMMC_CLOCK_INIT);

	/* Card need of 74 cycles clock minimum to start with CS HIGH. */
	sdmmc_send_dummy_clock(true);

	/* Reset card to idle state. */
	_retry = 10;
	do {
		_r1 = sdmmc_send_cmd(SDMMC_SPI_CMD0_GO_IDLE_STATE, 0);
		_retry--;
	}while((_r1 != R1_SPI_IDLE) && _retry);

	if(_r1 != R1_SPI_IDLE) {
		sdmmc_card.type = CARD_TYPE_UNKNOWN;
		SDMMC_RAW_DEBUGF("Reset card R1: 0x%X\n", _r1);
		return SD_MMC_ERR_COMM;
	}

	/* Try to initialize SD card version 2 or later. */
	if (sd_cmd8(&_ver2)) {
		return SD_MMC_ERR_UNHANDLE;
	}

	SDMMC_RAW_DEBUGF("Card Version2: %d\n", _ver2);

	/* Try to get the SDIO card's operating condition. */
	if (!sdio_op_cond()) {
		sdmmc_spi_deselect_device();
		return false;
	}

	if (sdmmc_card.type & CARD_TYPE_SD) {
		/* Try to get the SD card's operating condition. */
		SDMMC_RAW_DEBUGF("Check SD card...\n");
		if (sd_spi_op_cond(_ver2)) {
			/* It is not a SD card. */
			sdmmc_card.type = CARD_TYPE_MMC;
			return sdmmc_install_mmc();
		}

		/* The CRC on card is disabled by default.
		 * However, to be sure, the CRC OFF command is send.
		 * Unfortunately, specific SDIO card does not support it
		 * (H&D wireless card - HDG104 WiFi SIP)
		 * and the command is send only on SD card.
		 */
		_r1 = sdmmc_spi_send_cmd(SDMMC_SPI_CMD59_CRC_ON_OFF, 0);
		if (_r1) {
			SDMMC_RAW_DEBUGF("CMD59 R1: 0x%X\n", _r1);
			return SD_MMC_ERR_COMM;
		}
	}

	/* SD MEMORY. */
	if (sdmmc_card.type & CARD_TYPE_SD) {
		/* Get the Card-Specific Data. */
		if (sdmmc_cmd9_spi()) {
			return SD_MMC_ERR_COMM;
		}

		sd_decode_csd();
		/* Read the SCR to get card version. */
		SDMMC_RAW_DEBUGF("Read SCR...\n");
		if (sd_acmd51()) {
			return SD_MMC_ERR_COMM;
		}
	}
	if (IS_SDIO()) {
		if (!sdio_get_max_speed()) {
			return SD_MMC_ERR_COMM;
		}
	}

	/* SD MEMORY not HC, Set default block size. */
	if ((sdmmc_card.type & CARD_TYPE_SD) && (0 == (sdmmc_card.type & CARD_TYPE_HC))) {
		SDMMC_RAW_DEBUGF("Set Block Length...\n");
		_r1 = sdmmc_send_cmd(SDMMC_CMD16_SET_BLOCKLEN, SD_MMC_BLOCK_SIZE);
		if(_r1) {
			SDMMC_RAW_DEBUGF("CMD16 R1: 0x%X\n", _r1);
			return SD_MMC_ERR_COMM;
		}
	}

	/* Check communication. */
	SDMMC_RAW_DEBUGF("Check Communication...\n");
	if (sdmmc_card.type & CARD_TYPE_SD) {
		if (sdmmc_cmd13()) {
			return SD_MMC_ERR_COMM;
		}
	}
	/* Reinitialize the card with the new speed. */
	sdmmc_setup_card(sdmmc_card.clock);

	SDMMC_RAW_DEBUGF("Identify Card...\n");
	SDMMC_RAW_DEBUGF("Card Clock: %d\n", (int)sdmmc_card.clock);
	SDMMC_RAW_DEBUGF("Card Version: %d\n", sdmmc_card.version);
	return SD_MMC_OK;
}


static sdmmc_error_t sdmmc_install_mmc(void)
{
	uint8_t _r1, _retry, _b_authorize_high_speed;

	SDMMC_RAW_DEBUGF("Check MMC...\n");

	/* CMD0 - Reset all cards to idle state. */
	_retry = 10;
	do {
		_r1 = sdmmc_send_cmd(SDMMC_SPI_CMD0_GO_IDLE_STATE, 0);
		_retry--;
	}while((_r1 != R1_SPI_IDLE) && _retry);

	if (_r1 != R1_SPI_IDLE) {
		/* Unusable card. */
		SDMMC_RAW_DEBUGF("CMD0 R1: 0x%X\n", _r1);
		return SD_MMC_ERR_UNUSABLE;
	}

	if (mmc_spi_op_cond()) {
		/* Unusable card. */
		SDMMC_RAW_DEBUGF("MMC unusable\n");
		return SD_MMC_ERR_UNUSABLE;
	}

	/* Disable CRC check for SPI mode. */
	_r1 = sdmmc_send_cmd(SDMMC_SPI_CMD59_CRC_ON_OFF, 0);
	if (_r1) {
		SDMMC_RAW_DEBUGF("CMD59 R1: 0x%X\n", _r1);
		return SD_MMC_ERR_COMM;
	}

	/* Get the Card-Specific Data. */
	if (sdmmc_cmd9_spi()) {
		return SD_MMC_ERR_COMM;
	}

	mmc_decode_csd();
	/* For MMC 4.0 Higher version. */
	if (sdmmc_card.version >= CARD_VER_MMC_4) {
		/* Get EXT_CSD. */
		if (mmc_cmd8(&_b_authorize_high_speed)) {
			return SD_MMC_ERR_COMM;
		}
	}

	/* Set default block size. */
	_r1 = sdmmc_send_cmd(SDMMC_CMD16_SET_BLOCKLEN, SD_MMC_BLOCK_SIZE);
	if (_r1) {
		SDMMC_RAW_DEBUGF("CMD16 R1: 0x%X\n", _r1);
		return SD_MMC_ERR_COMM;
	}
	/* Check communication. */
	if (sdmmc_cmd13()) {
		return SD_MMC_ERR_COMM;
	}
	/* Reinitialize the card with the new speed. */
	sdmmc_setup_card(sdmmc_card.clock);
	SDMMC_RAW_DEBUGF("Card Clock: 0x%X\n", (int)sdmmc_card.clock);
	return SD_MMC_OK;
}

/* ------------------------------------------------------------------------------------------------ */

sdmmc_error_t sdmmc_init(void)
{
	sdmmc_error_t _retval;

	SDMMC_RAW_DEBUGF("SD/MMC Init...\n");

	sdmmc_card.state = SD_MMC_CARD_STATE_NO_CARD;
	/* Initialize SPI for SD/MMC. */
	sdmmc_spi_init();

	_retval = sdmmc_card_init();

	SDMMC_RAW_DEBUGF("Card Type: %d\n", sdmmc_card.type);

	return _retval;
}


sdmmc_error_t sdmmc_check(void)
{
	sdmmc_error_t sdmmc_err;

	sdmmc_err = sdmmc_check_card();

	SDMMC_RAW_DEBUGF("Check card: %d\n", sdmmc_err);

	if (sdmmc_err == SD_MMC_OK) {
		return sdmmc_err;
	}

	/* Initialization of the card requested. */
	if (sdmmc_card_init() == SD_MMC_OK) {
		sdmmc_card.state = SD_MMC_CARD_STATE_READY;
		/* To notify that the card has been just initialized. */
		/* It is necessary for USB Device MSC. */
		return SD_MMC_INIT_ONGOING;
	}

	sdmmc_card.state = SD_MMC_CARD_STATE_UNUSABLE;
	return SD_MMC_ERR_UNUSABLE;
}


card_type_t sdmmc_get_type(void)
{
	if (SD_MMC_OK != sdmmc_check_card()) {
		return CARD_TYPE_UNKNOWN;
	}
	return sdmmc_card.type;
}


card_version_t sdmmc_get_version(void)
{
	if (SD_MMC_OK != sdmmc_check_card()) {
		return CARD_VER_UNKNOWN;
	}
	return sdmmc_card.version;
}


uint32_t sdmmc_get_capacity(void)
{
	if (SD_MMC_OK != sdmmc_check_card()) {
		return 0;
	}
	return sdmmc_card.capacity;
}


void sdmmc_get_card_info(card_info_t *_card_info)
{
	_card_info->type = sdmmc_card.type;
	_card_info->version = sdmmc_card.version;
	_card_info->capacity = sdmmc_card.capacity;
	_card_info->clock = sdmmc_card.clock;
}


bool sdmmc_is_write_protected(void)
{
	if(!sdmmc_read_write_protect_pin()) {
		return true;
	}
	else {
		return false;
	}
}

sdmmc_error_t sdmmc_init_read_blocks(uint32_t _start, uint16_t _nb_block)
{
	sdmmc_error_t _sdmmc_err;
	uint32_t _cmd, _arg;
	uint8_t _r1;

	_sdmmc_err = sdmmc_check_card();
	if (_sdmmc_err != SD_MMC_OK) {
		return _sdmmc_err;
	}

	/* Wait for data ready status. */
	if (sdmmc_cmd13()) {
		return SD_MMC_ERR_COMM;
	}

	if (_nb_block > 1) {
		_cmd = SDMMC_CMD18_READ_MULTIPLE_BLOCK;
	} else {
		_cmd = SDMMC_CMD17_READ_SINGLE_BLOCK;
	}
	/*
	 * SDSC Card (CCS=0) uses byte unit address,
	 * SDHC and SDXC Cards (CCS=1) use block unit address (512 Bytes unit).
	 */
	if (sdmmc_card.type & CARD_TYPE_HC) {
		_arg = _start;
	}
	else {
		_arg = (_start * SD_MMC_BLOCK_SIZE);
	}

	_r1 = sdmmc_send_cmd(_cmd, _arg);
	if(_r1) {
		return SD_MMC_ERR_COMM;
	}

	/* Check response. */
	sdmmc_nb_block_remaining = _nb_block;
	sdmmc_nb_block_to_tranfer = _nb_block;
	return SD_MMC_OK;
}


sdmmc_error_t sdmmc_start_read_blocks(void *_dest, uint16_t _nb_block)
{
	if (sdmmc_read_block(_dest, _nb_block)) {
		sdmmc_nb_block_remaining = 0;
		return SD_MMC_ERR_COMM;
	}
	sdmmc_nb_block_remaining -= _nb_block;
	return SD_MMC_OK;
}


sdmmc_error_t sdmmc_wait_end_of_read_blocks(void)
{
	if(sdmmc_nb_block_remaining) {
		return SD_MMC_OK;
	}

	/* All blocks are transfered then stop read operation. */
	if (sdmmc_nb_block_to_tranfer == 1) {
		/* Single block transfer, then nothing to do. */
		return SD_MMC_OK;
	}

	/* Stop transmission. */
	sdmmc_send_cmd(SDMMC_CMD12_STOP_TRANSMISSION, 0);

	return SD_MMC_OK;
}


sdmmc_error_t sdmmc_init_write_blocks(uint32_t _start, uint16_t _nb_block)
{
	sdmmc_error_t _sdmmc_err;
	uint32_t _cmd, _arg;
	uint8_t _r1;

	_sdmmc_err = sdmmc_check_card();
	if (_sdmmc_err != SD_MMC_OK) {
		return _sdmmc_err;
	}
	if (sdmmc_is_write_protected()) {
		return SD_MMC_ERR_WP;
	}

	if (_nb_block > 1) {
		_cmd = SDMMC_CMD25_WRITE_MULTIPLE_BLOCK;
	} else {
		_cmd = SDMMC_CMD24_WRITE_BLOCK;
	}
	/*
	 * SDSC Card (CCS=0) uses byte unit address,
	 * SDHC and SDXC Cards (CCS=1) use block unit address (512 Bytes unit).
	 */
	if (sdmmc_card.type & CARD_TYPE_HC) {
		_arg = _start;
	}
	else {
		_arg = (_start * SD_MMC_BLOCK_SIZE);
	}

	_r1 = sdmmc_send_cmd(_cmd, _arg);
	if(_r1) {
		return SD_MMC_ERR_COMM;
	}

	sdmmc_nb_block_remaining = _nb_block;
	sdmmc_nb_block_to_tranfer = _nb_block;
	return SD_MMC_OK;
}


sdmmc_error_t sdmmc_start_write_blocks(const void *_src, uint16_t _nb_block)
{
	if (sdmmc_write_block(_src, _nb_block)) {
		sdmmc_nb_block_remaining = 0;
		return SD_MMC_ERR_COMM;
	}
	sdmmc_nb_block_remaining -= _nb_block;
	return SD_MMC_OK;
}


sdmmc_error_t sdmmc_wait_end_of_write_blocks(void)
{
	if (sdmmc_wait_end_of_write_block()) {
		return SD_MMC_ERR_COMM;
	}
	if (sdmmc_nb_block_remaining) {
		return SD_MMC_OK;
	}

	// All blocks are transfered then stop write operation
	if (sdmmc_nb_block_to_tranfer == 1) {
		// Single block transfer, then nothing to do
		return SD_MMC_OK;
	}

	return SD_MMC_OK;
}


#ifdef SDIO_SUPPORT_ENABLE
sdmmc_error_t sdio_read_direct(uint8_t func_num, uint32_t addr, uint8_t *dest)
{
	sdmmc_error_t sdmmc_err;

	if (dest == NULL) {
		return SD_MMC_ERR_PARAM;
	}

	sdmmc_err = sdmmc_check_card();
	if (sdmmc_err != SD_MMC_OK) {
		return sdmmc_err;
	}

	if (!sdio_cmd52(SDIO_CMD52_READ_FLAG, func_num, addr, 0, dest)) {
		return SD_MMC_ERR_COMM;
	}
	return SD_MMC_OK;
}

sdmmc_err_t sdio_write_direct(uint8_t func_num, uint32_t addr, uint8_t data)
{
	sdmmc_error_t sdmmc_err;

	sdmmc_err = sdmmc_check_card();
	if (sdmmc_err != SD_MMC_OK) {
		return sdmmc_err;
	}

	if (!sdio_cmd52(SDIO_CMD52_WRITE_FLAG, func_num, addr, 0, &data)) {
		return SD_MMC_ERR_COMM;
	}

	return SD_MMC_OK;
}

sdmmc_err_t sdio_read_extended(uint8_t func_num, uint32_t addr, int8_t inc_addr, uint8_t *dest, uint16_t size)
{
	sdmmc_error_t sdmmc_err;

	if ((size == 0) || (size > 512)) {
		return SD_MMC_ERR_PARAM;
	}

	sdmmc_err = sdmmc_check_card();
	if (sdmmc_err != SD_MMC_OK) {
		return sdmmc_err;
	}

	if (!sdio_cmd53(SDIO_CMD53_READ_FLAG, func_num, addr, inc_addr, size, true)) {
		return SD_MMC_ERR_COMM;
	}
	if (!sdmmc_spi_start_read_blocks(dest, 1)) {
		return SD_MMC_ERR_COMM;
	}
	if (!sdmmc_spi_wait_end_of_read_blocks()) {
		return SD_MMC_ERR_COMM;
	}

	return SD_MMC_OK;
}

sdmmc_error_t sdio_write_extended(uint8_t func_num, uint32_t addr, uint8_t inc_addr, uint8_t *src, uint16_t size)
{
	sdmmc_error_t sdmmc_err;

	if ((size == 0) || (size > 512)) {
		return SD_MMC_ERR_PARAM;
	}

	sdmmc_err = sdmmc_check_card();
	if (sdmmc_err != SD_MMC_OK) {
		return sdmmc_err;
	}

	if (!sdio_cmd53(SDIO_CMD53_WRITE_FLAG, func_num, addr, inc_addr, size, true)) {
		return SD_MMC_ERR_COMM;
	}
	if (!sdmmc_start_write_blocks(src, 1)) {
		return SD_MMC_ERR_COMM;
	}
	if (!sdmmc_spi_wait_end_of_write_blocks()) {
		return SD_MMC_ERR_COMM;
	}

	return SD_MMC_OK;
}
#endif // SDIO_SUPPORT_ENABLE
